﻿#include "cameracalibrator.h"
#include <QFile>
#include <QFileInfo>
#include <QDir>
#include "common_log.h"

CameraCalibrator::CameraCalibrator()
{
}
CameraCalibrator::~CameraCalibrator()
{
}
bool CameraCalibrator::InitCameraParam(const QString& path)
{
	cv::FileStorage fs;
	fs.open(path.toStdString(), cv::FileStorage::READ);
	fs["intrinsicRGB"] >> m_cameraMatri;
	fs["distortionDepth"] >> m_distCoeffs;
	fs.release();
	return true;
}
//3D点表转mat齐次矩阵表，多一列1
cv::Mat vvd3DPoint2MatfHom(const QVector<Position>& poss)
{
	cv::Mat MatTable = cv::Mat::ones(poss.size(), 3 + 1, CV_64FC1);

	for (int i = 0; i < poss.size(); i++)
	{
		MatTable.at<double>(i, 0) = poss.at(i).x;//float转double
		MatTable.at<double>(i, 1) = poss.at(i).y;
		MatTable.at<double>(i, 2) = poss.at(i).z;
	}

	return MatTable;
}

cv::Mat Get3DR_TransMatrix(const std::vector<cv::Point3f>& srcPoints, const std::vector<cv::Point3f>& dstPoints)
{
	double srcSumX = 0.0f;
	double srcSumY = 0.0f;
	double srcSumZ = 0.0f;

	double dstSumX = 0.0f;
	double dstSumY = 0.0f;
	double dstSumZ = 0.0f;

	//至少三组点
	if (srcPoints.size() != dstPoints.size() || srcPoints.size() < 3)
	{
		return cv::Mat();
	}

	int pointsNum = srcPoints.size();
	for (int i = 0; i < pointsNum; ++i)
	{
		srcSumX += srcPoints[i].x;
		srcSumY += srcPoints[i].y;
		srcSumZ += srcPoints[i].z;

		dstSumX += dstPoints[i].x;
		dstSumY += dstPoints[i].y;
		dstSumZ += dstPoints[i].z;
	}

	cv::Point3d centerSrc, centerDst;

	centerSrc.x = double(srcSumX / pointsNum);
	centerSrc.y = double(srcSumY / pointsNum);
	centerSrc.z = double(srcSumZ / pointsNum);

	centerDst.x = double(dstSumX / pointsNum);
	centerDst.y = double(dstSumY / pointsNum);
	centerDst.z = double(dstSumZ / pointsNum);

	//Mat::Mat(int rows, int cols, int type)
	cv::Mat srcMat(3, pointsNum, CV_64FC1);
	cv::Mat dstMat(3, pointsNum, CV_64FC1);

	for (int i = 0; i < pointsNum; ++i)//N组点
	{
		//三行
		srcMat.at<double>(0, i) = srcPoints[i].x - centerSrc.x;
		srcMat.at<double>(1, i) = srcPoints[i].y - centerSrc.y;
		srcMat.at<double>(2, i) = srcPoints[i].z - centerSrc.z;

		dstMat.at<double>(0, i) = dstPoints[i].x - centerDst.x;
		dstMat.at<double>(1, i) = dstPoints[i].y - centerDst.y;
		dstMat.at<double>(2, i) = dstPoints[i].z - centerDst.z;
	}

	cv::Mat matS = srcMat * dstMat.t();

	cv::Mat matU, matW, matV;
	cv::SVDecomp(matS, matW, matU, matV);

	cv::Mat matTemp = matU * matV;
	double det = cv::determinant(matTemp);//行列式的值

	double datM[] = { 1, 0, 0, 0, 1, 0, 0, 0, det };
	cv::Mat matM(3, 3, CV_64FC1, datM);

	cv::Mat matR = matV.t() * matM * matU.t();

	double* datR = (double*)(matR.data);
	double delta_X = centerDst.x - (centerSrc.x * datR[0] + centerSrc.y * datR[1] + centerSrc.z * datR[2]);
	double delta_Y = centerDst.y - (centerSrc.x * datR[3] + centerSrc.y * datR[4] + centerSrc.z * datR[5]);
	double delta_Z = centerDst.z - (centerSrc.x * datR[6] + centerSrc.y * datR[7] + centerSrc.z * datR[8]);

	//生成RT齐次矩阵(4*4)
	cv::Mat R_T = (cv::Mat_<double>(4, 4) <<
		matR.at<double>(0, 0), matR.at<double>(0, 1), matR.at<double>(0, 2), delta_X,
		matR.at<double>(1, 0), matR.at<double>(1, 1), matR.at<double>(1, 2), delta_Y,
		matR.at<double>(2, 0), matR.at<double>(2, 1), matR.at<double>(2, 2), delta_Z,
		0, 0, 0, 1
		);

	return R_T;
}

Position CameraCalibrator::BackProjectionPixToPhyXY(const Position& pix)
{
	float x = pix.x;
	float y = pix.y;
	if (!m_distCoeffs.empty())//如果含有形变矩阵需要矫正像素点  形变参数不是cv::Mat()
	{
		std::vector<cv::Point2f> vp2fPoints;
		cv::Point2f Point(x, y);
		vp2fPoints.push_back(Point);
		//矫正部分点
		cv::undistortPoints(vp2fPoints, vp2fPoints, m_cameraMatri, m_distCoeffs, cv::Mat(), m_cameraMatri);
		x = vp2fPoints[0].x;
		y = vp2fPoints[0].y;
	}

	//已知矫正后像素坐标PxPy与实际距离Z、相机内参与畸变，计算实际物理坐标XY
	//undistortPoints消除像素点畸变影响后，依据三角关系有：
	//x=fx*X/Z+cx  y=fy*Y/Z+cy
	//有：
	//X=(x-cx)*Z/fx   Y=(y-cy)*Z/fy
	float dfx = m_cameraMatri.at<float>(0, 0);
	float dfy = m_cameraMatri.at<float>(1, 1);
	float dcx = m_cameraMatri.at<float>(0, 2);
	float dcy = m_cameraMatri.at<float>(1, 2);

	Position result = {};
	result.x = (x - dcx) * pix.z / dfx;
	result.y = (y - dcy) * pix.z / dfy;
	result.z = pix.z;
	return result;
}

bool CameraCalibrator::Calculate(QVector<BDInfo>& datas)
{
	m_pix.clear();
	m_rob.clear();
	m_pix.resize(datas.size());
	m_rob.resize(datas.size());
	std::vector<cv::Point3f> cam3f;
	std::vector<cv::Point3f> rob3f;
	for (auto& iter : datas) {
		cv::Mat depth = cv::imread(iter.depth.toStdString(), -1);
		Position pix = {};
		pix.x = iter.pix_x;
		pix.y = iter.pix_y;
		for (int i = 0; i++; i < 10) {
			if (depth.at<float>(pix.y + i, pix.x + i) != 0.0f) {
				pix.z = depth.at<float>(pix.y + i, pix.x + i);
				break;
			}
			if (depth.at<float>(pix.y - i, pix.x - i) != 0.0f) {
				pix.z = depth.at<float>(pix.y - i, pix.x - i);
				break;
			}
		}
		if (pix.z == 0.0f) {
			continue;
		}
		Position pix_to_w = BackProjectionPixToPhyXY(pix);
		m_pix.append(pix_to_w);
		cv::Point3f cam = {};
		cam.x = pix_to_w.x;
		cam.y = pix_to_w.y;
		cam.z = pix_to_w.z;
		cam3f.push_back(cam);
		cv::Point3f rob = {};
		rob.x = iter.rob.x;
		rob.y = iter.rob.y;
		rob.z = iter.rob.z;
		rob3f.push_back(rob);
		m_rob.append(iter.rob);
	}

	cv::Mat matPaperInCam = Get3DR_TransMatrix(cam3f, rob3f);
	SaveResult(matPaperInCam);

	//反变换验证误差
	cv::Mat matPXHom = vvd3DPoint2MatfHom(m_pix);
	cv::Mat matPY = vvd3DPoint2MatfHom(m_rob);

	//利用标定结果投影出的机器人Y点
	cv::Mat matPY2 = matPaperInCam * matPXHom.t();
	matPY2 = matPY2.t();
	printf("仿射矩阵为:\n");
	std::cout << matPaperInCam << std::endl;
	//对比matPY与matPY2
	printf("-----------------------\n");
	printf("原始矩阵为:\n");
	std::cout << matPY << std::endl;
	printf("变换矩阵为:\n");
	printf("-----------------------\n");
	std::cout << matPY2 << std::endl;
	printf("-----------------------\n");
	printf("误差为：:\n");
	std::cout << matPY - matPY2 << std::endl;
	//------------------------------------
	return true;
}

bool CameraCalibrator::SetSavePath(const QString& path)
{
	m_path = path;
	return true;
}

void CameraCalibrator::SaveResult(const cv::Mat& exss)
{
	CalibrationInfo info = {};
	info.Intrinsic().fx() = m_cameraMatri.at<float>(0, 0);
	info.Intrinsic().fy() = m_cameraMatri.at<float>(1, 1);
	info.Intrinsic().cx() = m_cameraMatri.at<float>(0, 2);
	info.Intrinsic().cy() = m_cameraMatri.at<float>(1, 2);

	info.External().r00 = exss.at<double>(0, 0);
	info.External().r01 = exss.at<double>(0, 1);
	info.External().r02 = exss.at<double>(0, 2);
	info.External().t00 = exss.at<double>(0, 3);
	info.External().r10 = exss.at<double>(1, 0);
	info.External().r11 = exss.at<double>(1, 1);
	info.External().r12 = exss.at<double>(1, 2);
	info.External().t01 = exss.at<double>(1, 3);
	info.External().r20 = exss.at<double>(2, 0);
	info.External().r21 = exss.at<double>(2, 1);
	info.External().r22 = exss.at<double>(2, 2);
	info.External().t02 = exss.at<double>(2, 3);

	info.UnstackSpace().x_max = 2000;
	info.UnstackSpace().x_min = -2000;
	info.UnstackSpace().y_max = 2000;
	info.UnstackSpace().y_min = -2000;
	info.UnstackSpace().z_max = 2000;
	info.UnstackSpace().z_min = -2000;

	QString path = m_path + "/biaoding.json";
	QFile file(path);
	if (!file.exists()) {
		QFileInfo f(file.fileName());
		auto path = f.absoluteDir().dirName();
		if (!QDir().mkpath(path)) {
			log_error("无法创建文件， 保存失败", f.absoluteDir().dirName());
			return;
		}
	}
	QByteArray data = info.toByteArray(QJsonDocument::Indented);
	if (file.open(QIODevice::WriteOnly | QIODevice::Truncate)) {
		int ret = file.write(data);
		file.close();
	}
}
